using BepuUtilities.Memory;
using BepuUtilities;
using System.Runtime.CompilerServices;
using System;
using System.Diagnostics;
using BepuPhysics.CollisionDetection;
using BepuPhysics.Trees;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.Collidables
{
    public abstract class ShapeBatch
    {
        protected RawBuffer shapesData;
        protected int shapeDataSize;
        /// <summary>
        /// 获取批处理当前在不调整大小的情况下可以保留的形状数。
        /// </summary>
        public int Capacity { get { return shapesData.Length / shapeDataSize; } }
        protected BufferPool pool;
        protected IdPool idPool;
        /// <summary>
        /// 获取此批中形状类型的类型ID。
        /// </summary>
        public int TypeId { get; protected set; }
        /// <summary>
        /// 获取此形状批次的包含类型是否可能包含需要其他形状批次的子级。
        /// </summary>
        public bool Compound { get; protected set; }
        /// <summary>
        /// 获取存储在此批处理中的形状类型的大小(以字节为单位)。
        /// </summary>
        public int ShapeDataSize { get { return shapeDataSize; } }

        protected abstract void Dispose(int index, BufferPool pool);
        protected abstract void RemoveAndDisposeChildren(int index, Shapes shapes, BufferPool pool);

        public void Remove(int index)
        {
            idPool.Return(index, pool);
        }

        public void RemoveAndDispose(int index, BufferPool pool)
        {
            Dispose(index, pool);
            Remove(index);
        }

        public void RecursivelyRemoveAndDispose(int index, Shapes shapes, BufferPool pool)
        {
            RemoveAndDisposeChildren(index, shapes, pool);
            RemoveAndDispose(index, pool);
        }

        public abstract void ComputeBounds(ref BoundingBoxBatcher batcher);
        public abstract void ComputeBounds(int shapeIndex, in RigidPose pose, out Vector3 min, out Vector3 max);
        internal virtual void ComputeBounds(int shapeIndex, in Quaternion orientation, out float maximumRadius, out float maximumAngularExpansion, out Vector3 min, out Vector3 max)
        {
            throw new InvalidOperationException("Nonconvex shapes are not required to have a maximum radius or angular expansion implementation. This should only ever be called on convexes.");
        }
        public abstract void RayTest<TRayHitHandler>(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;
        public abstract void RayTest<TRayHitHandler>(int shapeIndex, in RigidPose rigidPose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler;

        /// <summary>
        /// 获取指向形状数据的原始非类型化指针。
        /// </summary>
        /// <param name="shapeIndex">要查找的形状的索引。</param>
        /// <param name="shapePointer">指向索引形状数据的指针。</param>
        /// <param name="shapeSize">形状数据的大小(字节)。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe void GetShapeData(int shapeIndex, out void* shapePointer, out int shapeSize)
        {
            Debug.Assert(shapeIndex >= 0 && shapeIndex < Capacity);
            shapePointer = shapesData.Memory + shapeDataSize * shapeIndex;
            shapeSize = shapeDataSize;
        }


        /// <summary>
        /// 释放所有形状槽,而不向池返回任何资源。
        /// </summary>
        public abstract void Clear();
        /// <summary>
        /// 如有必要,增加类型批次的大小以容纳目标容量。
        /// </summary>
        /// <param name="shapeCapacity">目标容量。</param>
        public abstract void EnsureCapacity(int shapeCapacity);
        /// <summary>
        /// 如果目标容量不同于当前容量,则更改类型批次的大小。请注意,缩小分配是保守的;调整大小将
        /// 决不允许现有形状指向未分配的内存。
        /// </summary>
        /// <param name="shapeCapacity">目标容量。</param>
        public abstract void Resize(int shapeCapacity);
        /// <summary>
        /// 将所有备份资源返回到池,使批处于不可用状态。
        /// </summary>
        public abstract void Dispose();

        /// <summary>
        /// 缩小或扩大批处理ID池的分配。请注意,缩小分配是保守的;调整大小永远不会允许丢失任何挂起的ID。
        /// </summary>
        /// <param name="targetIdCapacity">ID池中要为其分配空间的插槽数。</param>
        public void ResizeIdPool(int targetIdCapacity)
        {
            idPool.Resize(targetIdCapacity, pool);
        }

    }

    public abstract class ShapeBatch<TShape> : ShapeBatch where TShape : unmanaged, IShape
    {
        internal Buffer<TShape> shapes;

        /// <summary>
        /// 获取对与索引关联的形状的引用。
        /// </summary>
        /// <param name="shapeIndex">要检索的形状引用的索引。</param>
        /// <return>引用给定索引处的形状。</return>
        public ref TShape this[int shapeIndex] { get { return ref shapes[shapeIndex]; } }

        protected ShapeBatch(BufferPool pool, int initialShapeCount)
        {
            this.pool = pool;
            TypeId = default(TShape).TypeId;
            InternalResize(initialShapeCount, 0);
            idPool = new IdPool(initialShapeCount, pool);
        }

        // 请注意,形状不能移动;没有对使用它们的可碰撞对象的引用,因此我们无法更正它们的索引。
        // 但这很好-我们无论如何都不会直接迭代形状集。
        // (这并不意味着不可能压缩形状集-它只需要通过迭代可碰撞对象来做到这一点。)
        public int Add(in TShape shape)
        {
            var shapeIndex = idPool.Take();
            if (shapes.Length <= shapeIndex)
            {
                InternalResize(shapeIndex + 1, shapes.Length);
            }
            shapes[shapeIndex] = shape;
            return shapeIndex;
        }


        void InternalResize(int shapeCount, int oldCopyLength)
        {
            shapeDataSize = Unsafe.SizeOf<TShape>();
            var requiredSizeInBytes = shapeCount * Unsafe.SizeOf<TShape>();
            pool.TakeAtLeast(requiredSizeInBytes, out var newShapesData);
            var newShapes = newShapesData.As<TShape>();
#if DEBUG
            // 在调试模式下,未使用的插槽保持默认值。这有助于发现误用。
            if (newShapes.Length > shapes.Length)
                newShapes.Clear(shapes.Length, newShapes.Length - shapes.Length);
#endif
            if (shapesData.Allocated)
            {
                shapes.CopyTo(0, newShapes, 0, oldCopyLength);
                pool.Return(ref shapesData);
            }
            else
            {
                Debug.Assert(oldCopyLength == 0);
            }
            shapes = newShapes;
            shapesData = newShapesData;
        }

        public override void Clear()
        {
#if DEBUG
            shapes.Clear(0, idPool.HighestPossiblyClaimedId + 1);
#endif
            idPool.Clear();
        }
        public override void EnsureCapacity(int shapeCapacity)
        {
            if (shapes.Length < shapeCapacity)
            {
                InternalResize(shapeCapacity, idPool.HighestPossiblyClaimedId + 1);
            }
        }

        public override void Resize(int shapeCapacity)
        {
            shapeCapacity = BufferPool.GetCapacityForCount<TShape>(Math.Max(idPool.HighestPossiblyClaimedId + 1, shapeCapacity));
            if (shapeCapacity != shapes.Length)
            {
                InternalResize(shapeCapacity, idPool.HighestPossiblyClaimedId + 1);
            }
        }
        public override void Dispose()
        {
            Debug.Assert(shapesData.Id == shapes.Id, "If the buffer ids don't match, there was some form of failed resize.");
            pool.Return(ref shapesData);
            idPool.Dispose(pool);
        }
    }


    public class ConvexShapeBatch<TShape, TShapeWide> : ShapeBatch<TShape>
        where TShape : unmanaged, IConvexShape
        where TShapeWide : unmanaged, IShapeWide<TShape>
    {
        public ConvexShapeBatch(BufferPool pool, int initialShapeCount) : base(pool, initialShapeCount)
        {
        }

        protected override void Dispose(int index, BufferPool pool)
        {
            // 大多数具有关联宽类型的凸形形状没有任何内部资源要处理。
        }

        protected override void RemoveAndDisposeChildren(int index, Shapes shapes, BufferPool pool)
        {
            // 而且他们没有任何孩子。
        }

        public override void ComputeBounds(ref BoundingBoxBatcher batcher)
        {
            batcher.ExecuteConvexBatch(this);
        }

        public override void ComputeBounds(int shapeIndex, in RigidPose pose, out Vector3 min, out Vector3 max)
        {
            shapes[shapeIndex].ComputeBounds(pose.Orientation, out min, out max);
            min += pose.Position;
            max += pose.Position;
        }

        internal override void ComputeBounds(int shapeIndex, in Quaternion orientation, out float maximumRadius, out float angularExpansion, out Vector3 min, out Vector3 max)
        {
            ref var shape = ref shapes[shapeIndex];
            shape.ComputeBounds(orientation, out min, out max);
            shape.ComputeAngularExpansionData(out maximumRadius, out angularExpansion);
        }

        public override void RayTest<TRayHitHandler>(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler)
        {
            if (shapes[shapeIndex].RayTest(pose, ray.Origin, ray.Direction, out var t, out var normal) && t <= maximumT)
            {
                hitHandler.OnRayHit(ray, ref maximumT, t, normal, 0);
            }
        }

        public unsafe override void RayTest<TRayHitHandler>(int index, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler)
        {
            WideRayTester.Test<RaySource, TShape, TShapeWide, TRayHitHandler>(ref shapes[index], pose, ref rays, ref hitHandler);
        }
    }

    public class ConvexHullShapeBatch : ConvexShapeBatch<ConvexHull, ConvexHullWide>
    {
        public ConvexHullShapeBatch(BufferPool pool, int initialShapeCount) : base(pool, initialShapeCount)
        {
        }

        protected override void Dispose(int index, BufferPool pool)
        {
            shapes[index].Dispose(pool);
        }
    }


    public class HomogeneousCompoundShapeBatch<TShape, TChildShape, TChildShapeWide> : ShapeBatch<TShape> where TShape : unmanaged, IHomogeneousCompoundShape<TChildShape, TChildShapeWide>
        where TChildShape : IConvexShape
        where TChildShapeWide : IShapeWide<TChildShape>
    {
        public HomogeneousCompoundShapeBatch(BufferPool pool, int initialShapeCount) : base(pool, initialShapeCount)
        {
            Compound = true;
        }

        protected override void Dispose(int index, BufferPool pool)
        {
            shapes[index].Dispose(pool);
        }

        protected override void RemoveAndDisposeChildren(int index, Shapes shapes, BufferPool pool)
        {
            // 网格和其他单一类型容器没有任何形状注册的子项。
        }

        public override void ComputeBounds(ref BoundingBoxBatcher batcher)
        {
            batcher.ExecuteHomogeneousCompoundBatch(this);
        }

        public override void ComputeBounds(int shapeIndex, in RigidPose pose, out Vector3 min, out Vector3 max)
        {
            shapes[shapeIndex].ComputeBounds(pose.Orientation, out min, out max);
            min += pose.Position;
            max += pose.Position;
        }
        public override void RayTest<TRayHitHandler>(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler)
        {
            shapes[shapeIndex].RayTest(pose, ray, ref maximumT, ref hitHandler);
        }

        public override void RayTest<TRayHitHandler>(int shapeIndex, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler)
        {
            shapes[shapeIndex].RayTest(pose, ref rays, ref hitHandler);
        }
    }

    public class CompoundShapeBatch<TShape> : ShapeBatch<TShape> where TShape : unmanaged, ICompoundShape
    {
        Shapes shapeBatches;

        public CompoundShapeBatch(BufferPool pool, int initialShapeCount, Shapes shapeBatches) : base(pool, initialShapeCount)
        {
            this.shapeBatches = shapeBatches;
            Compound = true;
        }

        protected override void Dispose(int index, BufferPool pool)
        {
            shapes[index].Dispose(pool);
        }

        protected override void RemoveAndDisposeChildren(int index, Shapes shapes, BufferPool pool)
        {
            ref var shape = ref this.shapes[index];
            for (int i = 0; i < shape.ChildCount; ++i)
            {
                ref var child = ref shape.GetChild(i);
                shapes.RecursivelyRemoveAndDispose(child.ShapeIndex, pool);
            }
        }

        public override void ComputeBounds(ref BoundingBoxBatcher batcher)
        {
            batcher.ExecuteCompoundBatch(this);
        }

        public override void ComputeBounds(int shapeIndex, in RigidPose pose, out Vector3 min, out Vector3 max)
        {
            shapes[shapeIndex].ComputeBounds(pose.Orientation, shapeBatches, out min, out max);
            min += pose.Position;
            max += pose.Position;
        }

        public override void RayTest<TRayHitHandler>(int shapeIndex, in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler)
        {
            shapes[shapeIndex].RayTest(pose, ray, ref maximumT, shapeBatches, ref hitHandler);
        }

        public override void RayTest<TRayHitHandler>(int shapeIndex, in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler)
        {
            shapes[shapeIndex].RayTest(pose, ref rays, shapeBatches, ref hitHandler);
        }
    }



    public class Shapes
    {
        ShapeBatch[] batches;
        int registeredTypeSpan;

        // 请注意,并不是批次列表中的每个索引都能保证被填满。例如,如果只添加了一个圆柱体,并且该圆柱体的类型ID为7,
        // 则batches.Count和RegisteredTypeSpan将为8-但是索引0到6将为空。
        // 我们不倾向于在形状类型的批处理上进行任何性能敏感的迭代,所以这种不连续是很好的。
        public int RegisteredTypeSpan => registeredTypeSpan;

        public int InitialCapacityPerTypeBatch { get; set; }
        public ShapeBatch this[int typeIndex] => batches[typeIndex];
        BufferPool pool;


        public Shapes(BufferPool pool, int initialCapacityPerTypeBatch)
        {
            InitialCapacityPerTypeBatch = initialCapacityPerTypeBatch;
            // 除非发生非常奇怪的事情,否则这个列表几乎永远不会调整大小,而且由于批处理使用虚拟调用,我们必须允许存储引用类型。
            batches = new ShapeBatch[16];
            this.pool = pool;
        }

        /// <summary>
        /// 计算单个形状的边框。
        /// </summary>
        /// <param name="pose">计算的边框的姿势。</param>
        /// <param name="shapeIndex">形状的索引。</param>
        /// <param name="bound">具有指定姿势的指定形状的边框。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void UpdateBounds(in RigidPose pose, ref TypedIndex shapeIndex, out BoundingBox bounds)
        {
            // 注意：这里的最小和最大值以绝对坐标表示,这意味着在位置使用更高精度表示的情况下,这是一个必须更新的点。
            batches[shapeIndex.Type].ComputeBounds(shapeIndex.Index, pose, out bounds.Min, out bounds.Max);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ref TShape GetShape<TShape>(int shapeIndex) where TShape : unmanaged, IShape
        {
            var typeId = default(TShape).TypeId;
            return ref Unsafe.As<ShapeBatch, ShapeBatch<TShape>>(ref batches[typeId])[shapeIndex];
        }


        public TypedIndex Add<TShape>(in TShape shape) where TShape : unmanaged, IShape
        {
            var typeId = default(TShape).TypeId;
            if (RegisteredTypeSpan <= typeId)
            {
                registeredTypeSpan = typeId + 1;
                if (batches.Length <= typeId)
                {
                    Array.Resize(ref batches, typeId + 1);
                }
            }
            if (batches[typeId] == null)
            {
                batches[typeId] = default(TShape).CreateShapeBatch(pool, InitialCapacityPerTypeBatch, this);
            }

            Debug.Assert(batches[typeId] is ShapeBatch<TShape>);
            var batch = Unsafe.As<ShapeBatch, ShapeBatch<TShape>>(ref batches[typeId]);
            var index = batch.Add(shape);
            return new TypedIndex(typeId, index);
        }



        /// <summary>
        /// 从Shape集合中移除一个形状和任何现有子项,并将其资源返回给给定池。
        /// </summary>
        /// <param name="shapeIndex">要删除的形状的索引。</param>
        /// <param name="pool">要将所有形状资源返回到的池。</param>
        public void RecursivelyRemoveAndDispose(TypedIndex shapeIndex, BufferPool pool)
        {
            if (shapeIndex.Exists)
            {
                Debug.Assert(RegisteredTypeSpan > shapeIndex.Type && batches[shapeIndex.Type] != null);
                batches[shapeIndex.Type].RecursivelyRemoveAndDispose(shapeIndex.Index, this, pool);
            }
        }

        /// <summary>
        /// 从Shape集合中移除形状,并将其资源返回给给定池。不移除或处置任何子项。
        /// </summary>
        /// <param name="shapeIndex">要删除的形状的索引。</param>
        /// <param name="pool">要将所有形状资源返回到的池。</param>
        public void RemoveAndDispose(TypedIndex shapeIndex, BufferPool pool)
        {
            if (shapeIndex.Exists)
            {
                Debug.Assert(RegisteredTypeSpan > shapeIndex.Type && batches[shapeIndex.Type] != null);
                batches[shapeIndex.Type].RemoveAndDispose(shapeIndex.Index, pool);
            }
        }

        /// <summary>
        /// 在不移除其子形状或释放任何资源的情况下移除形状。
        /// </summary>
        /// <param name="shapeIndex">要删除的形状的索引。</param>
        public void Remove(TypedIndex shapeIndex)
        {
            if (shapeIndex.Exists)
            {
                Debug.Assert(RegisteredTypeSpan > shapeIndex.Type && batches[shapeIndex.Type] != null);
                batches[shapeIndex.Type].Remove(shapeIndex.Index);
            }
        }

        /// <summary>
        /// 从现有批次中清除所有形状。不释放任何内存。
        /// </summary>
        public void Clear()
        {
            for (int i = 0; i < registeredTypeSpan; ++i)
            {
                if (batches[i] != null)
                    batches[i].Clear();
            }
        }

        // 从技术上讲,我们这里缺少一些自由度,但这些主要是便利函数。底层批处理具有剩余的(很少使用的)功能。
        // 您可能还注意到,我们没有任何形式的每种类型的最小容量,就像我们在求解器中所做的那样。求解器受益于对分配进行更严格的"动态"控制
        // 因为类型批预计会相当频繁地创建和销毁-有时一帧会多次创建和销毁。接触约束来来去去,与用户输入无关。
        // 另一方面,形状只由用户添加或删除。
        /// <summary>
        /// 确保所有现有形状批次的最小容量。
        /// </summary>
        /// <param name="shapeCapacity">确保所有现有形状批次的容量。</param>
        public void EnsureBatchCapacities(int shapeCapacity)
        {
            for (int i = 0; i < registeredTypeSpan; ++i)
            {
                if (batches[i] != null)
                    batches[i].EnsureCapacity(shapeCapacity);
            }
        }

        /// <summary>
        /// 调整目标容量的所有现有批次的大小。请注意,这是保守的;它永远不会孤立现有形状。
        /// </summary>
        /// <param name="shapeCapacity">所有现有形状批次的目标容量。</param>
        public void ResizeBatches(int shapeCapacity)
        {
            for (int i = 0; i < registeredTypeSpan; ++i)
            {
                if (batches[i] != null)
                    batches[i].Resize(shapeCapacity);
            }
        }

        /// <summary>
        /// 从现有批处理中释放所有内存。使设置的形状处于不可用状态。
        /// </summary>
        public void Dispose()
        {
            for (int i = 0; i < registeredTypeSpan; ++i)
            {
                if (batches[i] != null)
                    batches[i].Dispose();
            }
        }
    }
}
